//
// Copyright (C) 2005 Andras Varga
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 2
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, see <http://www.gnu.org/licenses/>.
//

#include "inet/common/INETMath.h"
#include "inet/mobility/single/MyDynamicMotionFileCache.h"
#include "inet/mobility/single/MyDynamicMobility.h"

namespace inet {

Define_Module(MyDynamicMobility);

MyDynamicMobility::MyDynamicMobility()
{
    is3D = false;
    lines = nullptr;
    currentLine = -1;
    maxSpeed = 0;
}

void MyDynamicMobility::computeMaxSpeed()
{
    const MyDynamicMotionFile::Line& vec = *lines;
    double lastTime = vec[0];
    Coord lastPos(vec[1],vec[2],(is3D ? vec[3] : 0));
    unsigned int step = (is3D ? 4: 3);
    for (unsigned int i = step; i < vec.size(); i += step)
    {
        double elapsedTime = vec[i] - lastTime;
        Coord currPos(vec[i+1], vec[i+2], (is3D ? vec[i+3] : 0));
        double distance = currPos.distance(lastPos);
        double speed = distance / elapsedTime;
        if (speed > maxSpeed)
            maxSpeed = speed;
        lastPos.x = currPos.x;
        lastPos.y = currPos.y;
        lastPos.z = currPos.z;
        lastTime = vec[i];
    }
}

MyDynamicMobility::~MyDynamicMobility()
{
    MyDynamicMotionFileCache::deleteInstance();
}

void MyDynamicMobility::initialize(int stage)
{
    LineSegmentsMobilityBase::initialize(stage);

    EV_TRACE << "initializing BonnMotionMobility stage " << stage << endl;
    if (stage == INITSTAGE_LOCAL) {
        is3D = par("is3D");
        moveInterval = par("moveInterval");
        nodeId = par("nodeId");
        if (nodeId == -1)
            nodeId = getContainingNode(this)->getIndex();
        EV << "MyDynamicMobility get generated nodeId:"<< nodeId << endl;
        fname = par("traceFile");
        hostNum = par("hostNum");
        const MyDynamicMotionFile *mdFile = MyDynamicMotionFileCache::getInstance(hostNum)->getFile(fname);
        lines = mdFile->getLine(nodeId);
        const BonnMotionFile::Line& vec = *lines;
        std::cout << "initial: NodeID-" << vec[0] << " at "<< vec[1] << " , " << vec[2] << std::endl;
        if (!lines)
            throw cRuntimeError("Invalid nodeId %d -- no such line in file '%s'", nodeId, fname);
        oldVec = *lines;
        currentLine = 0;
        nextChange = 0;
        computeMaxSpeed();
    }
}

void MyDynamicMobility::setInitialPosition()
{
    //const BonnMotionFile::Line& vec = *lines;
    if (oldVec.size() >= 3) {
        lastPosition.x = oldVec[1];
        lastPosition.y = oldVec[2];
        lastPosition.z = is3D ? oldVec[3] : 0;
    }
}

void MyDynamicMobility::setTargetPosition()
{
    std::cout << "pre-Vector: NodeID-" << oldVec[0] << " at "<< oldVec[1] << " , " << oldVec[2] << std::endl;
    const MyDynamicMotionFile *mdFile = MyDynamicMotionFileCache::getInstance(0)->getFile(fname);
    lines = mdFile->getLine(nodeId);
    if (!lines){
        EV_WARN << "Warning: " << "Invalid nodeId %d -- no such line in file '%s'" << nodeId << fname << endl;
        //throw cRuntimeError("Invalid nodeId %d -- no such line in file '%s'", nodeId, fname);
        nextChange += moveInterval;
        targetPosition.x = oldVec[currentLine + 1];
        targetPosition.y = oldVec[currentLine + 2];
        targetPosition.z = is3D ? oldVec[currentLine + 3] : 0;
        currentLine = 0;
        return;
    }
    else{
        oldVec = *lines;
    }
    const BonnMotionFile::Line& vec = *lines;
    nextChange += moveInterval;
    targetPosition.x = vec[currentLine + 1];
    targetPosition.y = vec[currentLine + 2];
    targetPosition.z = is3D ? vec[currentLine + 3] : 0;
    currentLine = 0;
}

//void MyDynamicMobility::setTargetPosition()
//{
//    const MyDynamicMotionFile *mdFile = MyDynamicMotionFileCache::getInstance(0)->getFile(fname);
//    lines = mdFile->getLine(nodeId);
//            if (!lines)
//                throw cRuntimeError("Invalid nodeId %d -- no such line in file '%s'", nodeId, fname);
//    const BonnMotionFile::Line& vec = *lines;
////    if (currentLine + (is3D ? 3 : 2) >= (int)vec.size()) {
////        nextChange = -1;
////        stationary = true;
////        targetPosition = lastPosition;
////        return;
////    }
//
//    nextChange += moveInterval;
//    targetPosition.x = vec[currentLine + 1];
//    targetPosition.y = vec[currentLine + 2];
//    targetPosition.z = is3D ? vec[currentLine + 3] : 0;
//    currentLine = 0;
//}

void MyDynamicMobility::move()
{
    LineSegmentsMobilityBase::move();
    raiseErrorIfOutside();
}

void MyDynamicMobility::handleSelfMessage(cMessage *message)
{
//    lines = mdFile->getLine(nodeId);
//                if (!lines)
//                    throw cRuntimeError("Invalid nodeId %d -- no such line in file '%s'", nodeId, fname);
    moveAndUpdate();
    scheduleUpdate();
}

} // namespace inet

